#include "stm32h7xx_hal.h"
#include "w25q64_qspi.h"

#define W25Q64_CMD_READ_ID        0x9F
#define W25Q64_CMD_READ_DATA      0x6B
#define W25Q64_CMD_PAGE_PROGRAM   0x32
#define W25Q64_CMD_SECTOR_ERASE   0x20
#define W25Q64_CMD_WRITE_ENABLE   0x06
#define W25Q64_CMD_READ_STATUS    0x05

#define W25Q64_SECTOR_SIZE        4096
#define W25Q64_PAGE_SIZE          256

OSPI_HandleTypeDef hospi;

void MX_OCTOSPI1_Init(void)
{

  /* USER CODE BEGIN OCTOSPI1_Init 0 */

  /* USER CODE END OCTOSPI1_Init 0 */

  OSPIM_CfgTypeDef sOspiManagerCfg = {0};
//   HAL_OSPI_DLYB_CfgTypeDef HAL_OSPI_DLYB_Cfg_Struct = {0};

  /* USER CODE BEGIN OCTOSPI1_Init 1 */

  /* USER CODE END OCTOSPI1_Init 1 */
  /* OCTOSPI1 parameter configuration*/
  hospi.Instance = OCTOSPI1;
  hospi.Init.FifoThreshold = 4;
  hospi.Init.DualQuad = HAL_OSPI_DUALQUAD_DISABLE;
  hospi.Init.MemoryType = HAL_OSPI_MEMTYPE_MICRON;
  hospi.Init.DeviceSize = 24;
  hospi.Init.ChipSelectHighTime = 1;
  hospi.Init.FreeRunningClock = HAL_OSPI_FREERUNCLK_DISABLE;
  hospi.Init.ClockMode = HAL_OSPI_CLOCK_MODE_0;
  hospi.Init.WrapSize = HAL_OSPI_WRAP_NOT_SUPPORTED;
  hospi.Init.ClockPrescaler = 8;
  hospi.Init.SampleShifting = HAL_OSPI_SAMPLE_SHIFTING_HALFCYCLE;
  hospi.Init.DelayHoldQuarterCycle = HAL_OSPI_DHQC_DISABLE;
  hospi.Init.ChipSelectBoundary = 0;
  hospi.Init.DelayBlockBypass = HAL_OSPI_DELAY_BLOCK_BYPASSED;
  hospi.Init.MaxTran = 0;
  hospi.Init.Refresh = 0;
  if (HAL_OSPI_Init(&hospi) != HAL_OK)
  {
    while(1);
  }
  sOspiManagerCfg.ClkPort = 1;
  sOspiManagerCfg.NCSPort = 1;
  sOspiManagerCfg.IOLowPort = HAL_OSPIM_IOPORT_1_LOW;
  if (HAL_OSPIM_Config(&hospi, &sOspiManagerCfg, HAL_OSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
  {
    while(1);
  }
//   HAL_OSPI_DLYB_Cfg_Struct.Units = 0;
//   HAL_OSPI_DLYB_Cfg_Struct.PhaseSel = 0;
//   if (HAL_OSPI_DLYB_SetConfig(&hospi, &HAL_OSPI_DLYB_Cfg_Struct) != HAL_OK)
//   {
//     while(1);
//   }
  /* USER CODE BEGIN OCTOSPI1_Init 2 */

  /* USER CODE END OCTOSPI1_Init 2 */

}

uint32_t W25Q64_ReadID(void)
{
    uint8_t id[3] = {0};

    HAL_OSPI_Command(&hospi, &(OSPI_RegularCmdTypeDef){
        .OperationType = HAL_OSPI_OPTYPE_COMMON_CFG,
        .FlashId = HAL_OSPI_FLASH_ID_1,
        .InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE,
        .InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS,
        .Instruction = W25Q64_CMD_READ_ID,
        .AddressMode = HAL_OSPI_ADDRESS_NONE,
        .DataMode = HAL_OSPI_DATA_1_LINE,
        .NbData = 3,
        .DummyCycles = 0,
        .DQSMode = HAL_OSPI_DQS_DISABLE,
        .SIOOMode = HAL_OSPI_SIOO_INST_EVERY_CMD
    }, HAL_OSPI_TIMEOUT_DEFAULT_VALUE);
    
    HAL_OSPI_Receive(&hospi, id, HAL_OSPI_TIMEOUT_DEFAULT_VALUE);
    return (id[0] << 16) | (id[1] << 8) | id[2];
}

void W25Q64_Read(uint32_t address, uint8_t *pData, uint32_t size)
{
    HAL_OSPI_Command(&hospi, &(OSPI_RegularCmdTypeDef){
        .OperationType = HAL_OSPI_OPTYPE_COMMON_CFG,
        .FlashId = HAL_OSPI_FLASH_ID_1,
        .InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE,
        .InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS,
        .Instruction = W25Q64_CMD_READ_DATA,
        .AddressMode = HAL_OSPI_ADDRESS_1_LINE,
        .AddressSize = HAL_OSPI_ADDRESS_24_BITS,
        .Address = address,
        .DataMode = HAL_OSPI_DATA_4_LINES,
        .NbData = size,
        .DummyCycles = 8,
        .DQSMode = HAL_OSPI_DQS_DISABLE,
        .SIOOMode = HAL_OSPI_SIOO_INST_EVERY_CMD
    }, HAL_OSPI_TIMEOUT_DEFAULT_VALUE);
    
    HAL_OSPI_Receive(&hospi, pData, HAL_OSPI_TIMEOUT_DEFAULT_VALUE);
}

void W25Q64_WriteEnable(void)
{
    HAL_OSPI_Command(&hospi, &(OSPI_RegularCmdTypeDef){
        .OperationType = HAL_OSPI_OPTYPE_COMMON_CFG,
        .FlashId = HAL_OSPI_FLASH_ID_1,
        .InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE,
        .InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS,
        .Instruction = W25Q64_CMD_WRITE_ENABLE,
        .AddressMode = HAL_OSPI_ADDRESS_NONE,
        .DataMode = HAL_OSPI_DATA_NONE,
        .SIOOMode = HAL_OSPI_SIOO_INST_EVERY_CMD
    }, HAL_OSPI_TIMEOUT_DEFAULT_VALUE);
}

uint8_t W25Q64_ReadStatus(void)
{
    uint8_t status = 0;
    
    HAL_OSPI_Command(&hospi, &(OSPI_RegularCmdTypeDef){
        .OperationType = HAL_OSPI_OPTYPE_COMMON_CFG,
        .FlashId = HAL_OSPI_FLASH_ID_1,
        .InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE,
        .InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS,
        .Instruction = W25Q64_CMD_READ_STATUS,
        .AddressMode = HAL_OSPI_ADDRESS_NONE,
        .DataMode = HAL_OSPI_DATA_1_LINE,
        .NbData = 1,
        .SIOOMode = HAL_OSPI_SIOO_INST_EVERY_CMD
    }, HAL_OSPI_TIMEOUT_DEFAULT_VALUE);
    
    HAL_OSPI_Receive(&hospi, &status, HAL_OSPI_TIMEOUT_DEFAULT_VALUE);
    return status;
}

void W25Q64_WaitForWriteEnd(void)
{
    while (W25Q64_ReadStatus() & 0x01); // Wait for BUSY bit to clear
}

void W25Q64_SectorErase(uint32_t address)
{
    W25Q64_WriteEnable();
    
    HAL_OSPI_Command(&hospi, &(OSPI_RegularCmdTypeDef){
        .OperationType = HAL_OSPI_OPTYPE_COMMON_CFG,
        .FlashId = HAL_OSPI_FLASH_ID_1,
        .InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE,
        .InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS,
        .Instruction = W25Q64_CMD_SECTOR_ERASE,
        .AddressMode = HAL_OSPI_ADDRESS_1_LINE,
        .AddressSize = HAL_OSPI_ADDRESS_24_BITS,
        .Address = address,
        .DataMode = HAL_OSPI_DATA_NONE,
        .SIOOMode = HAL_OSPI_SIOO_INST_EVERY_CMD
    }, HAL_OSPI_TIMEOUT_DEFAULT_VALUE);
    
    W25Q64_WaitForWriteEnd();
}

void W25Q64_PageProgram(uint32_t address, uint8_t *pData, uint32_t size)
{
    W25Q64_WriteEnable();
    
    HAL_OSPI_Command(&hospi, &(OSPI_RegularCmdTypeDef){
        .OperationType = HAL_OSPI_OPTYPE_COMMON_CFG,
        .FlashId = HAL_OSPI_FLASH_ID_1,
        .InstructionMode = HAL_OSPI_INSTRUCTION_1_LINE,
        .InstructionSize = HAL_OSPI_INSTRUCTION_8_BITS,
        .Instruction = W25Q64_CMD_PAGE_PROGRAM,
        .AddressMode = HAL_OSPI_ADDRESS_1_LINE,
        .AddressSize = HAL_OSPI_ADDRESS_24_BITS,
        .Address = address,
        .DataMode = HAL_OSPI_DATA_4_LINES,
        .NbData = size,
        .SIOOMode = HAL_OSPI_SIOO_INST_EVERY_CMD
    }, HAL_OSPI_TIMEOUT_DEFAULT_VALUE);
    
    HAL_OSPI_Transmit(&hospi, pData, HAL_OSPI_TIMEOUT_DEFAULT_VALUE);
    W25Q64_WaitForWriteEnd();
}